/*
 * surveyorsvs.cpp
 *
 *  Created on: Aug 17, 2011
 *      Author: dlobato
 */

#include "surveyorsvs_driver.h"
#include <cstdio>
#include <signal.h>
#include <string>
#include <sensor_msgs/image_encodings.h>


SurveyorSVS_Driver::SurveyorSVS_Driver(ros::NodeHandle nh):
	svs_nh(nh),priv_nh("~"),
	svs_it(new image_transport::ImageTransport(svs_nh)),
	master_image_raw_pub(svs_it->advertiseCamera("master/image_raw", 1)),
	slave_image_raw_pub(svs_it->advertiseCamera("slave/image_raw", 1)),
	master_cinfo_mgr(svs_nh,"master"),
	slave_cinfo_mgr(svs_nh,"slave"),
	svs_dev(0) {

	//read params
	this->priv_nh.param("ip_addr", this->str_ip_addr, std::string("169.154.0.10"));
	this->priv_nh.param("master_port", this->master_port, 10001);
	this->priv_nh.param("slave_port", this->slave_port, 10002);
	std::string str_image_mode;
	this->priv_nh.param("image_mode", str_image_mode, std::string("RGB888_320x240"));//see surveyor.h svs_image_mode_enum

	//parse image mode
	if (str_image_mode.compare("RGB888_160x120") == 0){
		this->image_mode = RGB888_160x120;
	}else if (str_image_mode.compare("RGB888_320x240") == 0){
		this->image_mode = RGB888_320x240;
	}else if (str_image_mode.compare("RGB888_640x480") == 0){
		this->image_mode = RGB888_640x480;
	}else if (str_image_mode.compare("RGB888_1280x1024") == 0){
		this->image_mode = RGB888_1280x1024;
	}else{//default 320x240
		ROS_WARN_STREAM("image_mode=" << str_image_mode << "not supported. Using default mode RGB888_320x240");
		this->image_mode = RGB888_320x240;
	}

	//frame ids
	this->priv_nh.param("master_frame_id", this->master_frame_id, std::string("/svs_master_rgb_optical_frame"));
	this->priv_nh.param("slave_frame_id", this->slave_frame_id, std::string("/svs_slave_rgb_optical_frame"));
	//create svs instance
	this->svs_dev = svs_create(this->str_ip_addr.c_str(),this->master_port,this->slave_port);

	ROS_INFO_STREAM("Connecting to svs @ " << this->str_ip_addr << ":" << this->master_port << "," << this->slave_port);
	//init svs
	if (svs_init(this->svs_dev) < 0) {
		svs_destroy(this->svs_dev);
		this->svs_dev = 0;
		ROS_ERROR("failed to connect to svs");
		throw ros::Exception("failed to connect to svs");
	}
	if (svs_set_image_mode(&this->svs_dev->modules[SVS_MASTER], this->image_mode) < 0) {
		ROS_ERROR("failed to set image mode for SVS_MASTER module");
		throw ros::Exception("failed to set image mode");
	}
	if (svs_set_image_mode(&this->svs_dev->modules[SVS_SLAVE], this->image_mode) < 0) {
		ROS_ERROR("failed to set image mode for SVS_SLAVE module");
		throw ros::Exception("failed to set image mode");
	}
	ROS_INFO_STREAM("svs initialized");
}

SurveyorSVS_Driver::~SurveyorSVS_Driver() {
	if (svs_close(this->svs_dev)) {
		ROS_ERROR("failed to close svs connection");
	}
	svs_destroy(this->svs_dev);
	this->svs_dev = 0;
}

void SurveyorSVS_Driver::get_images(sensor_msgs::ImagePtr master_image,
									sensor_msgs::ImagePtr slave_image){
	if (svs_get_image(&this->svs_dev->modules[SVS_MASTER]) == 0){
		ROS_ERROR("failed to get image mode from SVS_MASTER module");
		throw ros::Exception("failed to get image");
	}

	master_image->header.frame_id = this->master_frame_id;
	master_image->header.stamp = ros::Time::now();
	master_image->height = this->svs_dev->modules[SVS_MASTER].image_mode.height;
	master_image->width = this->svs_dev->modules[SVS_MASTER].image_mode.width;
	master_image->encoding = sensor_msgs::image_encodings::RGB8;
	master_image->step = master_image->height * master_image->width *
			this->svs_dev->modules[SVS_MASTER].image_mode.channels;
	master_image->data.resize(this->svs_dev->modules[SVS_MASTER].image_buf_size);
	memcpy(&master_image->data[0],
			this->svs_dev->modules[SVS_MASTER].image_buf,
			this->svs_dev->modules[SVS_MASTER].image_buf_size);

	if (svs_get_image(&this->svs_dev->modules[SVS_SLAVE]) == 0) {
		ROS_ERROR("failed to get image mode from SVS_SLAVE module");
		throw ros::Exception("failed to get image");
	}

	slave_image->header.frame_id = this->slave_frame_id;
	slave_image->header.stamp = master_image->header.stamp;
	slave_image->height = this->svs_dev->modules[SVS_SLAVE].image_mode.height;
	slave_image->width = this->svs_dev->modules[SVS_SLAVE].image_mode.width;
	slave_image->encoding = sensor_msgs::image_encodings::RGB8;
	slave_image->step = slave_image->height * slave_image->width *
			this->svs_dev->modules[SVS_SLAVE].image_mode.channels;
	slave_image->data.resize(this->svs_dev->modules[SVS_SLAVE].image_buf_size);
	memcpy(&slave_image->data[0],
			this->svs_dev->modules[SVS_SLAVE].image_buf,
			this->svs_dev->modules[SVS_SLAVE].image_buf_size);
}

//return true if image and camerainfo sizes match
bool checkCameraInfo(const sensor_msgs::ImagePtr image, const sensor_msgs::CameraInfoPtr ci){
	return image->height == ci->height && image->width == ci->width;
}

void SurveyorSVS_Driver::publish_images(const sensor_msgs::ImagePtr master_image, const sensor_msgs::ImagePtr slave_image){

	sensor_msgs::CameraInfoPtr master_cinfo(new sensor_msgs::CameraInfo(this->master_cinfo_mgr.getCameraInfo()));
	sensor_msgs::CameraInfoPtr slave_cinfo(new sensor_msgs::CameraInfo(this->slave_cinfo_mgr.getCameraInfo()));

	//master camera info
	if (!checkCameraInfo(master_image, master_cinfo)){
		ROS_WARN_STREAM("image info and camera info don't match for SVS_MASTER (publishing uncalibrated data)");
		master_cinfo.reset(new sensor_msgs::CameraInfo());
		master_cinfo->height = master_image->height;
		master_cinfo->width = master_image->width;
	}
	master_cinfo->header = master_image->header;

	if (!checkCameraInfo(slave_image, slave_cinfo)){
		ROS_WARN_STREAM("image info and camera info don't match for SVS_SLAVE (publishing uncalibrated data)");
		slave_cinfo.reset(new sensor_msgs::CameraInfo());
		slave_cinfo->height = slave_image->height;
		slave_cinfo->width = slave_image->width;
	}
	slave_cinfo->header = slave_image->header;

	//publish
	this->master_image_raw_pub.publish(master_image,master_cinfo);
	this->slave_image_raw_pub.publish(slave_image,slave_cinfo);
}

void SurveyorSVS_Driver::poll(){
	sensor_msgs::ImagePtr master_image(new sensor_msgs::Image);
	sensor_msgs::ImagePtr slave_image(new sensor_msgs::Image);

	//images
	get_images(master_image, slave_image);
	publish_images(master_image, slave_image);

	//battery status FIXME
}
